clc;clear;close all;
addpath('Functions')
% filed limits [x_min x_max y_min y_max z_min z_max]
field_limits = [0 100 0 100 0 20];

if isfile("robot.mat")
    load("robot.mat");
    [ r_num,state_dim ] = size(robot);             % robot number,state dimensions for each robot
else
    r_num = 4;             % robot number
    state_dim = 3;          % state dimensions for each robot
    robot = [ rand(r_num,1)*field_limits(2), rand(r_num,1)*field_limits(4), rand(r_num,1)*field_limits(6) ];
    save robot.mat robot;
end
% compute the range of coverage for each robot at the x-y plane
discretization_num = 50; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

centers = [50 50]; c_index = 1;
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);
% integral = sum(R*mesh_size^2,'all')
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');

c = computeObj_vecform(robot,points_xy,R,mesh_size,r)           % initial cost

x0 = reshape(robot,numel(robot),1);
A = [ ];
b = [ ];
Aeq = [ ];
beq = [ ];
lb = [ones(r_num,1)*field_limits(1); ones(r_num,1)*field_limits(3); ones(r_num,1)*field_limits(5)];
ub =[ones(r_num,1)*field_limits(2); ones(r_num,1)*field_limits(4); ones(r_num,1)*field_limits(6)];
options = optimoptions('patternsearch','Display','iter','PlotFcn',@psplotbestf,'MaxIter',2000,'MeshTolerance',1*10^(-10));
[robot_new,fval] = patternsearch(@(x) computeObj_vecform(x,points_xy,R,mesh_size,r),x0,A,b,Aeq,beq,lb,ub,[],options);

c1 = computeObj_vecform(robot_new,points_xy,R,mesh_size,r)

%%
close all; 
load("robot.mat");field_limits = [0 100 0 100 0 20]; r_num = size(robot,1);
h = robot(:,end); 
r = coverageModel(h,20,'reciprocal');

f = figure('Name','SimultanuouslyCoverage3D'); hold on;
scatter3(robot(:,1),robot(:,2),robot(:,3),'filled'); grid on;
axis(field_limits);view(3);


%%%% plot shading %%%%%%%%%%%%%%%%%%%%%%%
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h2 = figure('Name','CoverageAreaBeforeOptimization'); hold on; grid on;
pcolor(X,Y,Z); shading interp
axis(field_limits(1:4));

% plotHandles_robot_gt = zeros(r_num,1);
% for i = 1:r_num  
%     x=robot(i,1)-r(i);
%     y=robot(i,2)-r(i);
%     plotHandles_robot_gt(i) = rectangle('Position',[x,y,2*r(i),2*r(i)],'Curvature',[1,1],'LineWidth',1);
% end

%%%%%%%%%%%%%%%%%%%%%       after optimization
robot = reshape(robot_new,numel(robot_new)/3,3);
figure(f); scatter3(robot(:,1),robot(:,2),robot(:,3),'filled','MarkerFaceColor' ,'r');
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h1 = figure('Name','CoverageAreaAfterOptimization'); hold on; grid on;
surf(X,Y,Z); shading interp;colormap default; colorbar
%contour(X,Y,Z,'ShowText','on')
axis(field_limits(1:4));
% plot the coverage area as a circle on the x-y plane
h = robot(:,end); 
r = coverageModel(h,20,'reciprocal');
% plotHandles_robot_gt = zeros(r_num,1);
% for i = 1:r_num  
%     x=robot(i,1)-r(i);
%     y=robot(i,2)-r(i);
%     plotHandles_robot_gt(i) = rectangle('Position',[x,y,2*r(i),2*r(i)],'Curvature',[1,1],'LineWidth',1);
% end
%figure(); surf(X_intep,Y_intep,Z_intep); colormap jet
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 
figure('Name','DensityDistribution'); dis_num = 200;
x_space = linspace(field_limits(1),field_limits(2),dis_num);
y_space = linspace(field_limits(3),field_limits(4),dis_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,dis_num^2,1) reshape(Y_points,dis_num^2,1)];
D = densityFunc([field_limits(2)/2 field_limits(4)/2],points_xy);
surf(X,Y,D); shading interp;colormap default; colorbar;